/**
 ******************************************************************************
 * @file           : bsp_mpu6050.c
 * @brief          : MPU6050 Six-Axis Sensor Driver Implementation
 ******************************************************************************
 */

/* Includes ------------------------------------------------------------------*/
#include "./Devices/MPU6050/bsp_mpu6050.h"
#include "./Devices/MPU6050/bsp_mpu6050_config.h"

/* Private includes ----------------------------------------------------------*/
#include <math.h>

/* Private define ------------------------------------------------------------*/
#define MPU6050_WHO_AM_I        0x75
#define MPU6050_PWR_MGMT_1      0x6B
#define MPU6050_SMPLRT_DIV      0x19
#define MPU6050_CONFIG          0x1A
#define MPU6050_GYRO_CONFIG     0x1B
#define MPU6050_ACCEL_CONFIG    0x1C
#define MPU6050_ACCEL_XOUT_H    0x3B
#define MPU6050_TEMP_OUT_H      0x41
#define MPU6050_GYRO_XOUT_H     0x43

/* Private variables ---------------------------------------------------------*/
static float sg_accel_scale = 16384.0f;  /* 加速度比例因子 */
static float sg_gyro_scale = 131.0f;     /* 陀螺仪比例因子 */

/* Private function prototypes -----------------------------------------------*/
static MPU6050_Error_e mpu6050_write_reg(uint8_t reg, uint8_t data);
static MPU6050_Error_e mpu6050_read_reg(uint8_t reg, uint8_t *data);
static MPU6050_Error_e mpu6050_read_bytes(uint8_t reg, uint8_t *buf, uint16_t len);

/* Exported functions --------------------------------------------------------*/

/**
 * @brief       初始化MPU6050
 */
MPU6050_Error_e mpu6050_init(void)
{
    uint8_t who_am_i;
    
    /* 读取WHO_AM_I寄存器 */
    if (mpu6050_read_reg(MPU6050_WHO_AM_I, &who_am_i) != MPU6050_OK)
    {
        log_e("MPU6050读取失败");
        return MPU6050_ERROR;
    }
    
    if (who_am_i != 0x68)
    {
        log_e("MPU6050未找到 WHO_AM_I=0x%02X", who_am_i);
        return MPU6050_ERROR_NOT_FOUND;
    }
    
    /* 复位MPU6050 */
    mpu6050_write_reg(MPU6050_PWR_MGMT_1, 0x80);
    HAL_Delay(100);
    
    /* 退出睡眠模式 */
    mpu6050_write_reg(MPU6050_PWR_MGMT_1, 0x00);
    HAL_Delay(10);
    
    /* 配置采样率分频 */
    mpu6050_write_reg(MPU6050_SMPLRT_DIV, 0x07);
    
    /* 配置低通滤波 */
    mpu6050_write_reg(MPU6050_CONFIG, 0x06);
    
    /* 配置加速度计量程 */
    mpu6050_set_accel_range(MPU6050_ACCEL_RANGE);
    
    /* 配置陀螺仪量程 */
    mpu6050_set_gyro_range(MPU6050_GYRO_RANGE);
    
    log_i("MPU6050初始化成功 ID=0x%02X", who_am_i);
    return MPU6050_OK;
}

/**
 * @brief       测试连接
 */
bool mpu6050_test_connection(void)
{
    uint8_t who_am_i;
    return (mpu6050_read_reg(MPU6050_WHO_AM_I, &who_am_i) == MPU6050_OK && who_am_i == 0x68);
}

/**
 * @brief       读取设备ID
 */
uint8_t mpu6050_read_id(void)
{
    uint8_t id;
    mpu6050_read_reg(MPU6050_WHO_AM_I, &id);
    return id;
}

/**
 * @brief       读取原始数据
 */
MPU6050_Error_e mpu6050_read_raw_data(MPU6050_Data_t *data)
{
    uint8_t buf[14];
    
    if (data == NULL)
    {
        return MPU6050_ERROR_PARAM;
    }
    
    /* 连续读取14字节（加速度6 + 温度2 + 陀螺仪6） */
    if (mpu6050_read_bytes(MPU6050_ACCEL_XOUT_H, buf, 14) != MPU6050_OK)
    {
        return MPU6050_ERROR;
    }
    
    data->accel_x = (int16_t)((buf[0] << 8) | buf[1]);
    data->accel_y = (int16_t)((buf[2] << 8) | buf[3]);
    data->accel_z = (int16_t)((buf[4] << 8) | buf[5]);
    data->temperature = (int16_t)((buf[6] << 8) | buf[7]);
    data->gyro_x = (int16_t)((buf[8] << 8) | buf[9]);
    data->gyro_y = (int16_t)((buf[10] << 8) | buf[11]);
    data->gyro_z = (int16_t)((buf[12] << 8) | buf[13]);
    
    return MPU6050_OK;
}

/**
 * @brief       读取物理值
 */
MPU6050_Error_e mpu6050_read_physical_data(MPU6050_PhysicalData_t *data)
{
    MPU6050_Data_t raw_data;
    
    if (data == NULL)
    {
        return MPU6050_ERROR_PARAM;
    }
    
    if (mpu6050_read_raw_data(&raw_data) != MPU6050_OK)
    {
        return MPU6050_ERROR;
    }
    
    /* 转换为物理值 */
    data->accel_x = (float)raw_data.accel_x / sg_accel_scale;
    data->accel_y = (float)raw_data.accel_y / sg_accel_scale;
    data->accel_z = (float)raw_data.accel_z / sg_accel_scale;
    
    data->gyro_x = (float)raw_data.gyro_x / sg_gyro_scale;
    data->gyro_y = (float)raw_data.gyro_y / sg_gyro_scale;
    data->gyro_z = (float)raw_data.gyro_z / sg_gyro_scale;
    
    data->temperature = (float)raw_data.temperature / 340.0f + 36.53f;
    
    return MPU6050_OK;
}

/**
 * @brief       读取温度
 */
MPU6050_Error_e mpu6050_read_temperature(float *temp)
{
    uint8_t buf[2];
    int16_t raw_temp;
    
    if (temp == NULL)
    {
        return MPU6050_ERROR_PARAM;
    }
    
    if (mpu6050_read_bytes(MPU6050_TEMP_OUT_H, buf, 2) != MPU6050_OK)
    {
        return MPU6050_ERROR;
    }
    
    raw_temp = (int16_t)((buf[0] << 8) | buf[1]);
    *temp = (float)raw_temp / 340.0f + 36.53f;
    
    return MPU6050_OK;
}

/**
 * @brief       设置加速度计量程
 */
MPU6050_Error_e mpu6050_set_accel_range(uint8_t range)
{
    uint8_t reg_val;
    
    switch (range)
    {
        case 2:  reg_val = 0x00; sg_accel_scale = 16384.0f; break;
        case 4:  reg_val = 0x08; sg_accel_scale = 8192.0f;  break;
        case 8:  reg_val = 0x10; sg_accel_scale = 4096.0f;  break;
        case 16: reg_val = 0x18; sg_accel_scale = 2048.0f;  break;
        default: return MPU6050_ERROR_PARAM;
    }
    
    return mpu6050_write_reg(MPU6050_ACCEL_CONFIG, reg_val);
}

/**
 * @brief       设置陀螺仪量程
 */
MPU6050_Error_e mpu6050_set_gyro_range(uint16_t range)
{
    uint8_t reg_val;
    
    switch (range)
    {
        case 250:  reg_val = 0x00; sg_gyro_scale = 131.0f;  break;
        case 500:  reg_val = 0x08; sg_gyro_scale = 65.5f;   break;
        case 1000: reg_val = 0x10; sg_gyro_scale = 32.8f;   break;
        case 2000: reg_val = 0x18; sg_gyro_scale = 16.4f;   break;
        default: return MPU6050_ERROR_PARAM;
    }
    
    return mpu6050_write_reg(MPU6050_GYRO_CONFIG, reg_val);
}

/* Private functions ---------------------------------------------------------*/

/**
 * @brief       写寄存器
 */
static MPU6050_Error_e mpu6050_write_reg(uint8_t reg, uint8_t data)
{
    uint8_t buf[2] = {reg, data};
    
#if (MPU6050_USE_BSP_I2C == 1)
    return (iic_write(MPU6050_I2C_ADDR, buf, 2) == IIC_OK) ? MPU6050_OK : MPU6050_ERROR;
#else
    if (HAL_I2C_Master_Transmit(MPU6050_I2C_HANDLE, MPU6050_I2C_ADDR, buf, 2, 100) == HAL_OK)
    {
        return MPU6050_OK;
    }
    return MPU6050_ERROR;
#endif
}

/**
 * @brief       读寄存器
 */
static MPU6050_Error_e mpu6050_read_reg(uint8_t reg, uint8_t *data)
{
#if (MPU6050_USE_BSP_I2C == 1)
    if (iic_write(MPU6050_I2C_ADDR, &reg, 1) != IIC_OK)
    {
        return MPU6050_ERROR;
    }
    return (iic_read(MPU6050_I2C_ADDR, data, 1) == IIC_OK) ? MPU6050_OK : MPU6050_ERROR;
#else
    if (HAL_I2C_Master_Transmit(MPU6050_I2C_HANDLE, MPU6050_I2C_ADDR, &reg, 1, 100) != HAL_OK)
    {
        return MPU6050_ERROR;
    }
    if (HAL_I2C_Master_Receive(MPU6050_I2C_HANDLE, MPU6050_I2C_ADDR, data, 1, 100) == HAL_OK)
    {
        return MPU6050_OK;
    }
    return MPU6050_ERROR;
#endif
}

/**
 * @brief       读多个字节
 */
static MPU6050_Error_e mpu6050_read_bytes(uint8_t reg, uint8_t *buf, uint16_t len)
{
#if (MPU6050_USE_BSP_I2C == 1)
    if (iic_write(MPU6050_I2C_ADDR, &reg, 1) != IIC_OK)
    {
        return MPU6050_ERROR;
    }
    return (iic_read(MPU6050_I2C_ADDR, buf, len) == IIC_OK) ? MPU6050_OK : MPU6050_ERROR;
#else
    if (HAL_I2C_Master_Transmit(MPU6050_I2C_HANDLE, MPU6050_I2C_ADDR, &reg, 1, 100) != HAL_OK)
    {
        return MPU6050_ERROR;
    }
    if (HAL_I2C_Master_Receive(MPU6050_I2C_HANDLE, MPU6050_I2C_ADDR, buf, len, 100) == HAL_OK)
    {
        return MPU6050_OK;
    }
    return MPU6050_ERROR;
#endif
}
